(defmethod pr2-interface
  (:move-to-feedback-test
   (cds1 cds2 &key (retry 10) (frame-id "/world") (wait-for-server-timeout 5))
   (let (ret (is_goaled nil) (is_reached nil) (count 0) (tm1 (ros::time-now))
	     (pose1 (ros::coords->tf-pose cds1))
	     (pose2 (ros::coords->tf-pose cds2))
             (goal1 (instance move_base_msgs::MoveBaseActionGoal :init))
             (goal2 (instance move_base_msgs::MoveBaseActionGoal :init)))
     (when (not (send move-base-action :wait-for-server wait-for-server-timeout))
       (return-from :move-to nil))
     ;;
     (send goal1 :header :stamp tm)
     (send goal1 :goal :target_pose :header :stamp tm)
     (send goal1 :goal :target_pose :header :frame_id frame-id)
     (send goal1 :goal :target_pose :pose pose1)

     (send move-base-action :send-goal goal1)
     (setq map-goal-coords (send (send *tfl* :lookup-tran8sform "/map" frame-id (ros::time 0)) :transform (send cds1 :copy-worldcoords)))

     (while (and (null is_goaled) (null is_reached)) ;; (<= (inc count) retry))
       ;; (if (> count 0)
       ;;    (clear-costmap))
       (ros::ros-info "move-to : send-goal to ~A (~d)" (send cds1 :worldpos) count)
       (ros::sleep) ;; (send move-base-action :wait-for-result)
       (if (eq (send move-base-action :get-state) actionlib_msgs::GoalStatus::*succeeded*)
	   (setq is_goaled t))

       (let (diff current-coords)
	 (when (null (send *tfl* :wait-for-transform "/map" "/base_footprint" (ros::time-now) 5))
	   (ros::ros-error ":move-to wait-for transform /map to /base_footprint failed")
	   (return-from :move-to-feedback nil))
	 (setq current-coords (send *tfl* :lookup-transform "/map" "/base_footprint" (ros::time 0)))
	 (setq diff (send current-coords :transformation map-goal-coords))
	 (if (< (norm (subseq (send diff :worldpos) 0 2)) 500)
	   (progn (ros::ros-warn "less than 200mm")
		  (setq is_reached t))
	   (ros::ros-warn "more than 200mm")))
       )
     (ros::ros-info "move-to : ~A" (if (or is_goaled is_reached) 'succeeded 'failed))
     (send move-base-action :cancel-all-goals) ;; not needed?
     (unix:usleep (* 1000 100))
     (let (ret (tm2 (ros::time-now)))
       (send goal1e :header :stamp tm2)
       (send goal2 :goal :target_pose :header :stamp tm2)
       (send goal2 :goal :target_pose :header :frame_id frame-id)
       (send goal2 :goal :target_pose :pose pose2)
       (send move-base-action :send-goal goal2)
       (send move-base-action :wait-for-result)
       (if (eq (send move-base-action :get-state) actionlib_msgs::GoalStatus::*succeeded*)
	 (setq ret t))
       (ros::ros-info "move-to : ~A" (if ret 'succeeded 'failed)))
     ))
  (:go-pos-unsafe-sequence
   ()
   )
  (:move-trajectory-sequence
   (li &optionoal (msec 5000) &key (stop t))
   (ros::spin-once)
   (let ((odom-pos (scale 0.001 (send (send self :state :odom :pose) :pos)))
	 (odom-angle (elt (car (send (send self :state :odom :pose) :rpy-angle)) 0))
	 (msg (instance trajectory_msgs::JointTrajectory :init))
	 (goal (instance pr2_controllers_msgs::JointTrajectoryActionGoal :init))
	 ;;pt1 -  the current position of PR2
	 (pt1 (instance trajectory_msgs::JointTrajectoryPoint :init)))

     (send pt1 :time_from_start (ros::time))
     (send pt1 :positions (v+ odom-pos
			      (float-vector 0 0 odom-angle)))
     (send pt1 :velocities (rotate-vector (float-vector x y d) odom-angle :z))

     (let ((pt (instance trajectory_msgs::JointTrajectoryPoint :init))
	   (sec (/ msec 1000.0))
	   )

       (send pt2 :time_from_start (ros::time sec))
       (send pt2 :positions (v+ (v+ odom-pos
				    (float-vector 0 0 (+ odom-angle (* sec d))))
				(rotate-vector (scale sec (float-vector x y 0))
					       odom-angle :z)))
       )
     (if stop
	 (send pt2 :velocities (float-vector 0 0 0)) ;; To stop just
       (send pt2 :velocities (rotate-vector (float-vector x y d) odom-angle :z)))
     ;;
     (send msg :header :stamp (ros::time-now))
     (send msg :joint_names (list "base_link_x" "base_link_y" "base_link_pan"))
	 (send msg :points (list pt1 pt2))
     (send goal :goal :trajectory msg)
     ;;
     goal))
  ;;
  )